package src.bojan;

import java.awt.Color;
import java.util.Collection;
import java.util.Set;

import sim.engine.SimState;
import sim.engine.Steppable;
import sim.field.continuous.Continuous2D;
import sim.portrayal.simple.OvalPortrayal2D;
import sim.util.Double2D;
import bojan.robots.motion.DistanceEvaluator;
import bojan.robots.motion.Driver;
import bojan.robots.motion.Motion;
import bojan.robots.motion.MotionValidator;

public class Navigator {

	Continuous2D grid;

	private TerrainMap map;
	private TerrainMap realMap;
	private Driver driver;

	private Double2D previousPosition;

	private DistanceEvaluator distanceEvaluator;

	private MotionValidator motionValidator;

	public Navigator(Continuous2D grid, Double2D initPosition, TerrainMap map,
			TerrainMap realMap, Driver driver,
			DistanceEvaluator distanceEvalutor,
			MotionValidator motionValidor) {
		super();
		this.grid = grid;
		this.driver = driver;
		this.map = map;
		this.realMap = realMap;
		this.distanceEvaluator = distanceEvalutor;
		this.motionValidator = motionValidor;
		changePosition(initPosition);
		scan(initPosition);
	}

	public boolean reachGoal(Double2D goal, double maxCost) {
		double cost = 0;
		while (!(goal.equals(getPosition())) && cost <= maxCost - 1) { // TODO 1
																		// should
																		// be a
																		// param
			move(goal);
			cost++;
		}
		return goal.equals(getPosition());
	}

	public void move(Double2D goal) {
		if (goal.equals(getPosition())) {
			return;
		}
		Set<Double2D> alternatives = driver.nextPosition(getPosition(), goal);
		Double2D nPos = choosePosition(goal, alternatives);
		changePosition(nPos);
		scan(nPos);
	}

	public Double2D choosePosition(Double2D goal, Set<Double2D> alternatives) {
		if (alternatives.size() > 1) {
			alternatives.remove(previousPosition);
		}
	
		return motionValidator.correct(distanceEvaluator.closest(alternatives, goal));
	}

	private void changePosition(Double2D newPosition) {
		previousPosition = getPosition();
		grid.setObjectLocation(this, newPosition);
	}

	public void scan(Double2D standingPosition) {
		
		Collection<Double2D> unkowns = map.visibleUnknowns(standingPosition);
		for (Double2D visible : unkowns) {
			map.map(visible, (Known) realMap.mapping(visible));
		}
	}

	public Double2D getPosition() {
		return grid.getObjectLocation(this);
	}

	

}
